import json
import heapq
import requests

def load(filename):
    with open(filename,'r',encoding='UTF-8') as file:
        data = json.load(file)
    return data

info = r'Car-Robot\data\scene_info.json'
map_location = r'Car-Robot\data\map_location.json'
navi_point = r'Car-Robot\data\navi_point.json'
path = r'Car-Robot\data\path.json'
robot =r'Car-Robot\data\robot_loc.json'

class NaviFunction:



    '''这部分是路径规划函数，主要是使用了Dijkstra算法进行路径规划'''
    
    def dijkstra_shortest_path(self,start, end, graph):
        predecessors = {}
        # 初始化距离字典
        distances = {node: float('infinity') for node in graph}
        distances[start] = 0
    
        # 初始化优先队列
        pq = [(0, start)]
    
        while pq:
            current_distance, current_node = heapq.heappop(pq)
            
            if current_node == end:
                path = []
                while current_node in predecessors:
                    path.insert(0, current_node)
                    current_node = predecessors[current_node]
                path.insert(0, start)
                return path
            
            if current_distance > distances[current_node]:
                continue
            
            for neighbor, weight in graph[current_node].items():
                distance = current_distance + weight
                if distance < distances[neighbor]:
                    distances[neighbor] = distance
                    heapq.heappush(pq, (distance, neighbor))
                    predecessors[neighbor] = current_node
        
        return None
    
    

    def multi_dikstra_shortest_path(self,robot_loc, dst_loc,graph):
        path_dic = {}
        for i in range(len(robot_loc)):
            path = self.dijkstra_shortest_path(robot_loc[i], dst_loc[i], graph)
            path_dic[robot_loc[i]] = path
        return path_dic
    
    def car_command_gen(self,path_dic):
        start_robot = 106
        command_list = []
        for i in range(5):
            payload = {
                "robotId": start_robot+i,
                "command": "chas-backward {0} {1} 1.0 1".format(self.point_dic[path_dic[i][0]][0],self.point_dic[path_dic[i][0]][1]),
            }
            command_list.append(payload)
        return command_list
            


class RobotMove:
    def control(self,payload):
        url = 'http://110.42.175.97:8001/rcs/api/guest/648a1e0c53ececb7e1fc260ba5a47959/robot-control/control'
        

        response = requests.post(url, json=payload)

        if response.status_code == 200:
            data = response.json()
            if data["ok"]:
                print("Operation successful")
            else:
                print("Operation failed. Error message: {}".format(data["msg"]))
        else:
            print("Failed to connect to the server")



    def status(self):
        url = 'http://110.42.175.97:8001/rcs/api/guest/648a1e0c53ececb7e1fc260ba5a47959/robot-control/status?sceneid=189'
        params = {
            "sceneid": 189
        }

        response = requests.get(url, params=params)

        if response.status_code == 200:
            data = response.json()
            if data["ok"]:
                status_list = data["statuslist"]
                for status in status_list:
                    print("Robot ID: {}".format(status["robotId"]))
                    print("Position X: {}".format(status["posX"]))
                    print("Position Y: {}".format(status["posY"]))
                    print("Linear Speed: {}".format(status["linearSpeed"]))
                    print("Angular Speed: {}".format(status["angularSpeed"]))
                    print("Chas Orientation: {}".format(status["chasOrientation"]))
                    print("Board Orientation: {}".format(status["boardOrientation"]))
                    print("Status Name: {}".format(status["statusName"]))
                    print("Task ID: {}".format(status["taskid"]))
                    print("\n")
            else:
                print("Operation failed. Error message: {}".format(data["msg"]))
        else:
            print("Failed to connect to the server")

        return response.json()


